#include "Model.h"

using std::cout;
using std::endl;

namespace MMM
{

Model::Model()
	: name()
{
	mass = 1.0f;
	height = 1.0f;
}

Model::Model(const std::string &type, const std::string &name)
    : name(name), type(type)
{
	mass = 1.0f;
	height = 1.0f;
}

std::string Model::toXML()
{
	// convert model to Simox/VirtualRobot compatible XML format

	std::stringstream ss;
	//ss << "<?xml version='1.0' encoding='UTF-8'?>" << endl << endl;
	ss << "<Robot Type='" << name << "' RootNode='" << rootNode<< "'>" << endl << endl;
	for (auto & modelNode : modelNodes)
	{
		ss << modelNode->toXML() << endl;
	}
	ss << endl;
	ss << "</Robot>" << endl;

	return ss.str();
}

void Model::print()
{
	std::string s = toXML();
	cout << s << endl;
}

void Model::setRoot(const std::string &name)
{
	rootNode = name;
}

std::string Model::getRoot()
{
	return rootNode;
}


void Model::setMass(float massKG)
{
	mass = massKG;
}

void Model::setHeight(float heightM)
{
	height = heightM;
}

float Model::getHeight()
{
	return height;
}

float Model::getMass()
{
	return mass;
}

bool Model::addModelNode( ModelNodePtr n )
{
	if (!n)
	{
		MMM_ERROR << "NULL node data" << endl;
		return false;
	}
	if (n->name.empty())
	{
		MMM_ERROR << "Could not add node with empty name!" << endl;
		return false;
	}

	for (size_t i=0;i<modelNodes.size();i++)
	{
		if (modelNodes[i]->name == n->name)
		{
			MMM_ERROR << "Could not add node with name " << n->name << ". Node " << i << " has already the same name..." << endl;
			return false;
		}
	}
	modelNodes.push_back(n);
	return true;
}

bool Model::addModelNodeSet(ModelNodeSet ns)
{
    modelNodeSets.push_back(ns);
    return true;
}

void Model::setName(const std::string &name)
{
	this->name = name;
}

std::string Model::getName()
{
	return name;
}

void Model::setType(const std::string &type)
{
	this->type = type;
}

std::string Model::getType()
{
	return type;
}

std::vector<ModelNodePtr> Model::getModels()
{
	return modelNodes;
}

std::vector<ModelNodeSet> Model::getModelNodeSets()
{
    return modelNodeSets;
}

ModelPtr Model::clone()
{
	ModelPtr res(new Model(type,name));
	res->setMass(mass);
	res->setHeight(height);
	res->filename = filename;
	for (auto & modelNode : modelNodes)
	{
		ModelNodePtr m = modelNode->clone();
		res->addModelNode(m);
	}
	res->setRoot(rootNode);

    BOOST_FOREACH(ModelNodeSet mns, getModelNodeSets())
    {
        res->addModelNodeSet(mns);
    }


	return res;
}

std::string Model::getFilename()
{
	return filename;
}

bool Model::hasMarker(const std::string &markerName) const
{
	std::vector<ModelNodePtr>::const_iterator it = modelNodes.begin();

	while (it != modelNodes.end())
	{
		if ((*it)->hasMarker(markerName))
			return true;
		it++;
	}
	return false;
}

ModelNodePtr Model::getModelNode(const std::string& nodeName)
{
    for (auto & modelNode : modelNodes)
    {
        if (modelNode->name == nodeName)
            return modelNode;
    }

    return ModelNodePtr();
}


}
